﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using SlimMath;

namespace M3G {
    public class KeyframeSequence : Object3D {
        #region Constant

        // 補間方法
        public const int Linear       = 175;
        public const int Slerp        = 177;
        public const int Spline       = 178;
        public const int Squad        = 179;
        public const int Step         = 180;

        // リピート・モード
        public const int Constant     = 192;
        public const int Loop         = 193;
        public const int AdditiveLoop = 194;
        #endregion

        struct Keyframe {
            public int time;
            public float[,] values;  // [channel,component]
        }
        struct Event {
            public Event (int time, int id) {
                this.time = time;
                this.id = id;
            }
            public int time;
            public int id;
        }

        #region Field
        int        keyframeCount;
        int        componentCount;
        int        channelCount;
        int        interpolation;
        int        repeatMode;
        int        validRangeFirst;
        int        validRangeLast;
        int        duration;
        Keyframe[]   keyframes;
        List<Event>  events;
        #endregion

        #region Method
        public KeyframeSequence (int numKeyframes, int numComponents, int interpolation)
            : this (1, numKeyframes, numComponents, interpolation) {

        }

        public KeyframeSequence (int numChannels, int numKeyframes, int numComponents, int interpolation) {
            if (numChannels < 1) {
                throw new ArgumentException ("Number of channels must be greater than 0, num=" + numChannels);
            }
            if (interpolation < Linear || interpolation > Step) {
                throw new ArgumentException ("Interpolation type must be Linear,Slerp,Spline,Squad,Step");
            }
            if (numKeyframes < 1) {
                throw new ArgumentException ("Number of keyframes must be grater than 0, num=" + numKeyframes);
            }
            if (numComponents < 1) {
                throw new ArgumentException ("Number of components must be greater than 0, num=" + numComponents);
            }
            // ArgumentException - if interpolation is not a valid interpolation mode for keyframes of size numComponents

            this.keyframeCount = numKeyframes;
            this.componentCount = numComponents;
            this.channelCount = numChannels;
            this.interpolation = interpolation;
            this.repeatMode = Constant;
            this.validRangeFirst = 0;
            this.validRangeLast = numKeyframes - 1;

            this.keyframes = new Keyframe[numKeyframes];
            for (int i = 0; i < numKeyframes; i++) {
                this.keyframes[i].values = new float[numChannels, numComponents];
            }
            this.events = new List<Event> ();
        }

        protected KeyframeSequence (KeyframeSequence keyframeSequence)
            : base (keyframeSequence) {
                this.keyframeCount = keyframeSequence.keyframeCount;
                this.componentCount = keyframeSequence.componentCount;
                this.channelCount = keyframeSequence.channelCount;
                this.interpolation = keyframeSequence.interpolation;
                this.repeatMode = keyframeSequence.repeatMode;
                this.validRangeFirst = keyframeSequence.validRangeFirst;
                this.validRangeLast = keyframeSequence.validRangeLast;
                this.duration = keyframeSequence.duration;
                this.keyframes = (Keyframe[])keyframeSequence.keyframes.Clone ();
                this.events = new List<Event> (keyframeSequence.events);
        }

        public void AddEvent (int time, int eventID) {
            if (time < 0) {
                throw new ArgumentException ("Time must be within [0,inf], time=" + time);
            }
            if (eventID == 0) {
                throw new ArgumentException ("EventID must not be 0");
            }

            events.Add (new Event (time, eventID));
            events.Sort ((x, y) => x.time - y.time);
        }


        public int ChanelCount {
            get { return channelCount; }
        }

        public int ComponentCount {
            get { return componentCount; }
        }

        public int Duration {
            get { return duration; }
            set {
                var duration = value;
                if (duration < 0) {
                    throw new ArgumentException ("Duration must be within [0,inf], duration=" + duration);
                }

                this.duration = duration;
            }
        }

        public int EventCount {
            get { return events.Count (); }
        }

        public int[] EventID {
            get { return events.ConvertAll (e => e.id).ToArray (); }
        }

        public int[] EventTime {
            get { return events.ConvertAll (e => e.time).ToArray (); }
        }

        public int InterpolationType {
            get { return interpolation; }
        }


        public int GetKeyframe (int index, float[] value) {
            return GetKeyframe (0, index, value);
        }

        public int GetKeyframe (int channel, int index, float[] value) {
            if (channel < 0 || channel >= channelCount) {
                throw new ArgumentOutOfRangeException ("Channel index must be within [0," + (channelCount - 1) + "]");
            }
            if (index < 0 || index >= keyframeCount) {
                throw new ArgumentOutOfRangeException ("Keyframe index must be within [0," + (keyframeCount - 1) + "]");
            }
            if (value != null && value.Length < componentCount) {
                throw new ArgumentException ("Value is less than required, len=" + value.Length + "<" + componentCount);
            }

            if (value != null) {
                for (var i=0; i < componentCount; i++) {
                    value[i] = keyframes[index].values[channel, i];
                }
            }
            return keyframes[index].time;
        }


        public int KeyframeCount {
            get { return keyframeCount; }
        }

        public int RepeatMode {
            get { return repeatMode; }
            set {
                int mode = value;
                if (mode < Constant || mode > AdditiveLoop) {
                    throw new ArgumentException ("Mode must be one of Constant,Loop,AdditiveLoop defined in KeyframeSequence");
                }
                if (mode == AdditiveLoop && (interpolation == Slerp || interpolation == Squad)) {
                    throw new InvalidOperationException ("Additive loop must not be used with Slerp,Squad");
                }

                this.repeatMode = mode;
            }
        }

        public void RemoveEvent (int eventIndex) {
            if (eventIndex < 0 || eventIndex >= EventCount) {
                throw new ArgumentOutOfRangeException ("Event index must be within [0," + eventIndex + "]");
            }

            events.RemoveAt (eventIndex);
        }

        public float Sample (float time, int channel) {
            var value = new float[1];
            Sample (time, channel, value);
            return value[0];
        }

        public void Sample (float time, int channel, float[] value) {
            if (value == null) {
                throw new ArgumentNullException ("Value is null");
            }
            if (channel < 0 || channel >= channelCount) {
                throw new ArgumentOutOfRangeException ("Channel must be within [0," + (channelCount - 1) + "]");
            }
            if (value.Length < componentCount) {
                throw new ArgumentException ("Length of value is less than required, len=" + value.Length + ",req=" + componentCount);
            }
            if (duration == 0) {
                throw new InvalidOperationException ("Duration is not set");
            }
            if (duration < keyframes[validRangeLast].time) {
                throw new InvalidOperationException ("Duration must be larger than time of last valid keyframe, dura=" + duration);
            }
            for (var i=validRangeFirst + 1; i <= validRangeLast; i++) {
                if (keyframes[i % keyframeCount].time < keyframes[(i - 1) % keyframeCount].time) {
                    throw new InvalidOperationException ("Time of all keyframes must be increasing order");
                }
            }

            int n, l, ll, r, rr;

            n = CountLoop (ref time);
            SeekLeftToRight (out l, out ll, time);
            SeekRightToLeft (out r, out rr, time);

            switch (interpolation) {
                case Step: InterpolateStep (ll, l, r, rr, n, time, channel, value); break;
                case Linear: InterpolateLinear (ll, l, r, rr, n, time, channel, value); break;
                case Spline: InterpolateSpline (ll, l, r, rr, n, time, channel, value); break;
                case Slerp: InterpolateSlerp (ll, l, r, rr, n, time, channel, value); break;
                case Squad: InterpolateSquad (ll, l, r, rr, n, time, channel, value); break;
                default: break;
            }

            if (repeatMode == AdditiveLoop) {
                AddLoopValue (n, channel, value);
            }
        }

        void AddLoopValue (int n, int channel, float[] value) {
            for (var i=0; i < componentCount; i++) {
                var d = keyframes[validRangeLast].values[channel, i] - keyframes[validRangeFirst].values[channel, i];
                value[i] = value[i] + n * d;
            }
        }

        int CountLoop (ref float time) {
            if (repeatMode == Constant) {
                return 0;
            }

            int n = 0;
            while (time < 0 || time > duration) {
                if (time > duration) {
                    time -= duration;
                    n++;
                }
                if (time < 0) {
                    time += duration;
                    n--;
                }

            }
            return n;
        }

        /**
         * forループはFindXXXとdelegateを使って
         * もう少しきれいに書けないか? さすがにこれは
         * */
        void SeekLeftToRight (out int l, out int ll, float time) {

            // 仮想キーフレーム番号。
            // 必ず first < last
            var first = validRangeFirst;
            var last  = (validRangeLast < validRangeFirst) ? validRangeLast + keyframeCount : validRangeLast;

            // 指定時刻がすべてのキーフレームより大きく
            // 下のforループで値が入らない時のl,ll
            l = last;
            ll = last - 1;

            // 左から右に探索して指定時刻を超えたキーフレームの１つ左側のキーフレームをlとする
            // 指定時刻と正確に一致する時刻を持つキーフレームが存在する場合はそれをlとする
            for (var i = first; i <= last; i++) {
                if (keyframes[i % keyframeCount].time > time) {
                    l = i - 1;
                    ll = i - 2;
                    break;
                }
                if (keyframes[i % keyframeCount].time == time) {
                    l = i;
                    ll = i - 1;
                    break;
                }
            }

            // 折り返し処理[first,last]
            if (repeatMode == Constant) {
                l = (l < first) ? first : l;
                ll = (ll < first) ? first : ll;
            }
            if (repeatMode == Loop || repeatMode == AdditiveLoop) {
                l = (l < first) ? l + last - first + 1 : l;
                ll = (ll < first) ? ll + last - first + 1 : ll;
            }

            // 物理キーフレーム番号にマップ[0,keyframeCount-1]
            l %= keyframeCount;
            ll %= keyframeCount;
        }

        void SeekRightToLeft (out int r, out int rr, float time) {

            // 仮想キーフレーム番号。
            // 必ず first < last
            var first = validRangeFirst;
            var last  = (validRangeLast < validRangeFirst) ? validRangeLast + keyframeCount : validRangeLast;

            // 指定時刻がすべてのキーフレームより小さく
            // 下のforループで値が入らない時のr,rr
            r = first;
            rr = first + 1;

            // 左から右に探索して指定時刻を下回ったキーフレームの１つ右側のキーフレームをrとする
            // 指定時刻と正確に一致する時刻を持つキーフレームが存在する場合はそれをrとする
            for (var i = last; i >= first; i--) {
                if (keyframes[i % keyframeCount].time < time) {
                    r = i + 1;
                    rr = i + 2;
                    break;
                }
                if (keyframes[i % keyframeCount].time == time) {
                    r = i;
                    rr = i + 1;
                    break;
                }
            }

            // 折り返し処理[first,last]
            if (repeatMode == Constant) {
                r = (r > last) ? last : r;
                rr = (rr > last) ? last : rr;
            }
            if (repeatMode == Loop || repeatMode == AdditiveLoop) {
                r = (r > last) ? r - (last - first + 1) : r;
                rr = (rr > last) ? rr - (last - first + 1) : rr;
            }

            // 物理キーフレーム番号にマップ[0,keyframeCount-1]
            r %= keyframeCount;
            rr %= keyframeCount;
        }

        void InterpolateStep (int ll, int l, int r, int rr, int n, float time, int channel, float[] value) {
            if (keyframes[l].time == keyframes[r].time) {
                l = r;
            }

            for (var i=0; i < componentCount; i++) {
                value[i] = keyframes[l].values[channel, i];
            }
        }

        void InterpolateLinear (int ll, int l, int r, int rr, int n, float time, int channel, float[] value) {

            // 仮想キーフレームの時刻
            // ここで必ず tl < time < tr にする
            var tl = (keyframes[l].time > time) ? keyframes[l].time - duration : keyframes[l].time;
            var tr = (keyframes[r].time < time) ? keyframes[r].time + duration : keyframes[r].time;

            var s = (tl == tr) ? 1.0f : (time - tl) / (tr - tl);

            for (var i=0; i < componentCount; i++) {
                var leftValue  = keyframes[l].values[channel, i];
                var rightValue = keyframes[r].values[channel, i];

                value[i] = leftValue * (1 - s) + rightValue * s;
            }
        }

        void InterpolateSpline (int ll, int l, int r, int rr, int n, float time, int channel, float[] value) {
            // 仮想キーフレームの時刻
            // ここで必ず tll < tl < time < tr < trr にする
            var tll = (keyframes[ll].time > time) ? keyframes[ll].time - duration : keyframes[ll].time;
            var tl  = (keyframes[l].time > time) ? keyframes[l].time - duration : keyframes[l].time;
            var tr  = (keyframes[r].time < time) ? keyframes[r].time + duration : keyframes[r].time;
            var trr = (keyframes[rr].time < time) ? keyframes[rr].time + duration : keyframes[rr].time;

            var s = (tl == tr) ? 1.0f : (time - tl) / (tr - tl);

            for (var i = 0; i < componentCount; i++) {

                var vll = keyframes[ll].values[channel, i];
                var vl  = keyframes[l].values[channel, i];
                var vr  = keyframes[r].values[channel, i];
                var vrr = keyframes[rr].values[channel, i];

                var Tl = (vr - vll) / 2.0f;
                var Tr = (vrr - vl) / 2.0f;

                var Fl_minus = (repeatMode == Constant) ? 0 : 2 * (tr - tl) / (tr - tll);
                var Fr_plus  = (repeatMode == Constant) ? 0 : 2 * (tr - tl) / (trr - tl);

                var Tl0 =  Fl_minus * Tl;
                var Tr1 =  Fr_plus * Tr;

                var a = 2 * vl - 2 * vr + Tl0 + Tr1;
                var b = -3 * vl + 3 * vr - 2 * Tl0 - Tr1;
                var c = Tl0;
                var d = vl;

                value[i] = a * (s * s * s) + b * (s * s) + c * (s) + d;
            }

        }

        void InterpolateSlerp (int ll, int l, int r, int rr, int n, float time, int channel, float[] value) {
            // 仮想キーフレームの時刻
            // ここで必ず tll < tl < time < tr < trr にする
            var tl  = (keyframes[l].time > time) ? keyframes[l].time - duration : keyframes[l].time;
            var tr  = (keyframes[r].time < time) ? keyframes[r].time + duration : keyframes[r].time;

            var s = (tl == tr) ? 1.0f : (time - tl) / (tr - tl);

            var q1 = new Quaternion (keyframes[l].values[channel, 0], keyframes[l].values[channel, 1], keyframes[l].values[channel, 2], keyframes[l].values[channel, 3]);
            var q2 = new Quaternion (keyframes[r].values[channel, 0], keyframes[r].values[channel, 1], keyframes[r].values[channel, 2], keyframes[r].values[channel, 3]);

            var q3 = Quaternion.Slerp (q1, q2, 0);

            value[0] = q3[0];
            value[1] = q3[1];
            value[2] = q3[2];
            value[3] = q3[3];

        }

        /**
         * (注意) この実装は手抜き。
         * SlimMathのSquadは一様な間隔でサンプリングされたQuaternionのsquadにしか対応していない
         * 本当はTangentベクターの補正項Fが必要。詳しくは仕様書。
         * しばらくはこのまま手抜きで
         * */
        void InterpolateSquad (int ll, int l, int r, int rr, int n, float time, int channel, float[] value) {
            // 仮想キーフレームの時刻
            // ここで必ず tll < tl < time < tr < trr にする
            var tll  = (keyframes[ll].time > time) ? keyframes[ll].time - duration : keyframes[ll].time;
            var tl   = (keyframes[l].time > time) ? keyframes[l].time - duration : keyframes[l].time;
            var tr   = (keyframes[r].time < time) ? keyframes[r].time + duration : keyframes[r].time;
            var trr  = (keyframes[rr].time < time) ? keyframes[rr].time + duration : keyframes[rr].time;

            var s = (tl == tr) ? 1.0f : (time - tl) / (tr - tl);

            var q1 = new Quaternion (keyframes[ll].values[channel, 0], keyframes[ll].values[channel, 1], keyframes[ll].values[channel, 2], keyframes[ll].values[channel, 3]);
            var q2 = new Quaternion (keyframes[l].values[channel, 0], keyframes[l].values[channel, 1], keyframes[l].values[channel, 2], keyframes[l].values[channel, 3]);
            var q3 = new Quaternion (keyframes[r].values[channel, 0], keyframes[r].values[channel, 1], keyframes[r].values[channel, 2], keyframes[r].values[channel, 3]);
            var q4 = new Quaternion (keyframes[rr].values[channel, 0], keyframes[rr].values[channel, 1], keyframes[rr].values[channel, 2], keyframes[rr].values[channel, 3]);

            var q5 = Quaternion.Squad (q1, q2, q3, q4, s);

            value[0] = q5[0];
            value[1] = q5[1];
            value[2] = q5[2];
            value[3] = q5[3];
        }


        public void SetKeyframe (int index, int time, float[] value) {
            SetKeyframeTime (index, time);
            SetKeyframeValue (0, index, value);
        }

        public void SetKeyframeTime (int index, int time) {
            if (index < 0 || index >= keyframeCount) {
                throw new ArgumentOutOfRangeException ("Index must be within [0," + (keyframeCount - 1) + "]");
            }
            if (time < 0) {
                throw new ArgumentException ("Time must be positive number");
            }

            this.keyframes[index].time = time;

        }

        public void SetKeyframeValue (int channel, int index, float[] value) {
            if (value == null) {
                throw new ArgumentNullException ("Value is null");
            }
            if (index < 0 || index >= keyframeCount) {
                throw new ArgumentOutOfRangeException ("Index must be in [0," + (keyframeCount - 1) + "]");
            }
            if (channel < 0 || channel >= channelCount) {
                throw new ArgumentOutOfRangeException ("Channel must be in [0," + (channelCount - 1) + "]");
            }
            if (value.Length < componentCount) {
                throw new ArgumentException ("Length of value is less than required, len=" + value.Length + "<" + componentCount);
            }

            for (var i = 0; i < componentCount; i++) {
                this.keyframes[index].values[channel, i] = value[i];
            }
        }

        public int ValidRangeFirst {
            get { return validRangeFirst; }
        }

        public int ValidRangeLast {
            get { return validRangeLast; }
        }

        public void SetValidRange (int first, int last) {
            if (first < 0 || first > keyframeCount) {
                throw new ArgumentOutOfRangeException ("Valid range first must be in [0," + (keyframeCount - 1) + "]");
            }
            if (last < 0 || last > keyframeCount) {
                throw new ArgumentOutOfRangeException ("Valid range last must be in [0," + (keyframeCount - 1) + "]");
            }

            this.validRangeFirst = first;
            this.validRangeLast = last;
        }

        public override Object3D Duplicate () {
            return new KeyframeSequence(this);
        }

        #endregion



    }
}
